#ifndef _L_BV_TREE_120104_H
#define _L_BV_TREE_120104_H

#include <ltypes.h>
#include <lmacros.h>
#include <lmath.h>
#include <lvectors.h>
#include <ltriMesh.h>

#include <boost/assert.hpp>
#include <boost/noncopyable.hpp>

#include <vector>
#include <deque>

LLIB_BEGIN
namespace geom {

#pragma region class forward declarations
class BVTreeIter;
class BVTreeIter_Preorder;
class BVTreeIter_Inorder;
class BVTreeIter_Postorder;
class BVTree;
class BV_AABB;
class NormalCone;
class BVNode;
#pragma endregion

#pragma region class BVTreeIter

/* The abstract BVTree iterator class */
class BVTreeIter {
public:
    /* Here we define the traversal type for the factory method */
    enum TraversalType { Preorder = 0, Inorder = 1, Postorder = 2, Hierarchy = 3, LeafOnly = 4 };

    /* The factory method for build types of iterator on demand */
    static BVTreeIter* Create( BVTree& tree, TraversalType type);

    /* Restart the iterator from the beginning */
    inline void Restart() { reset(); }

    /* Get the next BVNode, return NULL when reach the end */
    virtual BVNode* Next() = 0;

protected:
    /* Hide default constructor */
    BVTreeIter() {}

    /* Reset internal states for a new traversal */
    virtual void reset() = 0;

    /* The BVTree pointer */
    BVTree* _tree;

};

/* The Preorder traversal BVTree iterator class */
class BVTreeIter_Preorder : public BVTreeIter {
public:
    virtual BVNode* Next();
protected:
    virtual void reset();
private:
    /* The preorder traversal helper stack */
    std::deque<BVNode*> _stack;
};

/* The Inorder traversal BVTree iterator class */
class BVTreeIter_Inorder : public BVTreeIter {
public:
    virtual BVNode* Next();
protected:
    virtual void reset();
private:
    /* The preorder traversal helper stack */
    std::deque<BVNode*> _stack;
};

/* The Postorder traversal BVTree iterator class */
class BVTreeIter_Postorder : public BVTreeIter {
public:
    virtual BVNode* Next();
protected:
    virtual void reset();
private:
    /* The preorder traversal helper stack */
    std::deque<BVNode*> _stack;
    BVNode* _lastPop;
};

#pragma endregion

#pragma region class BVTree

/* Here we define a bounding volume hierarchy data structure.
 * Specifically, we build up a complete binary tree, which can be
 * saved in array and indexed easily.
 */
class BVTree : public boost::noncopyable {
public:
    /* Constructor */
    BVTree() : _mesh(NULL), _nodeNum(0), _nodeList(NULL), _leafNum(0), _leafIndexList(NULL), _firstLeafIndex(-1), _lastLeafIndex(-1) {}
    /* Destructor
       The destructor need to free all tree nodes.
     */
    ~BVTree();

    /* Initializer : input the corresponding mesh and build up tree */
    void Init(TriMesh& triMesh);

    /* !!!Discard!!! */
    inline void Build(const std::vector<Vec3f>& elementPostions) { build(elementPostions); }

    /* The hierarchy node number query */
    inline Int32 GetNodeNum() const { return _nodeNum; }
    inline Int32 GetLeafNum() const { return _leafNum; }

    /* Get the root node pointer */
    inline BVNode* GetRoot() const { return _nodeList; }

    /* Create a BVTree traversal iterator */
    inline BVTreeIter* CreateIter(BVTreeIter::TraversalType type) { return BVTreeIter::Create(*this, type); }

private:
    /* The pointer of triangle mesh with tree is built on */
    TriMesh*    _mesh;

    /* The tree nodes array list */
    Int32       _nodeNum;
    BVNode*     _nodeList;

    /* The corresponding triangle index of leaf nodes */
    Int32       _leafNum;
    Int32*      _leafIndexList;

    /* The first and last index of leaf within _nodeList */
    Int32       _firstLeafIndex;
    Int32       _lastLeafIndex;

    /* Building hierarchy according to elements' center position */
    void    build(const std::vector<Vec3f>& elementPostions);
    void    leafArrangement(const std::vector<Vec3f>& elementPostions, std::vector<Int32>& leafIndex) const;
    Int32   completeBinaryTreeLeafDivide(Int32 leafNum) const;

    /* The friends */
    friend BVNode;
};

#pragma endregion

#pragma region class BV_AABB & NormalCone

/* The Axis Aligned Bounding Box */
class BV_AABB {
public:
    /* The default constructor */
    BV_AABB() {}

    /* The constructor with data */
    BV_AABB(const Vec3f& max, const Vec3f& min) : _max(max), _min(min) {}

    /* Update bounding volunm */
    inline void Update(const Vec3f& max, const Vec3f& min) { _max = max; _min = min; }

    /* Merge */
    inline BV_AABB Merge(const BV_AABB& another) const { return Merge(*this, another); }
    static BV_AABB Merge(const BV_AABB& first, const BV_AABB& second);

    /* Intersect */
    inline Boolean Intersect(const BV_AABB& another) const { return Intersect(*this, another); }
    static Boolean Intersect(const BV_AABB& first, const BV_AABB& second);

private:
    Vec3f _max;
    Vec3f _min;
};

class NormalCone {
public:
    /* The default constructor */
    NormalCone() {}

    /* The constructor with data */
    NormalCone(const Vec3f& direction, Float angle) : _direction(direction), _angle(angle) {}

    /* Update normal cone */
    inline void Update(const Vec3f& direction, Float angle) { _direction = direction; _angle = angle; }

    /* Merge */
    inline NormalCone Merge(const NormalCone& another) const { return Merge(*this, another); }
    static NormalCone Merge(const NormalCone& first, const NormalCone& second);

    /* Validate */
    inline Boolean Validate() const { return _angle < math::PI_2; }
    inline static Boolean Validate(const NormalCone& nc) { return nc._angle < math::PI_2; }

private:
    /* The direction vector, note: it must be normal */
    Vec3f _direction;

    /* The angle */
    Float _angle;

};

#pragma endregion

#pragma region class BVNode

/* The BVTree node */
class BVNode {
public:
    /* Root node check */
    inline Boolean IsRoot() const { return this == _tree->_nodeList; }

    /* Leaf node check */
    inline Boolean IsLeaf() const { return isLeaf(); }

    /* Get the corresponding mesh face of current leaf node */
    MeshFace GetLeafMeshFace() const;

    /* Get the depth of current node in tree
     * Note: the depth of root is 0
     */
    Int32 GetDepth() const;

    /* !!!Discard!!! */
    inline Int32 GetElemIndex() const { return _tree->_leafIndexList[ getLeafIndex() ]; }

    /* Parent node query
     * Note: return NULL when current node is root.
     */
    inline BVNode* GetParent() const {
        if(IsRoot()) return NULL;
        Int32 index = (getNodeIndex() - 1) / 2;
        return _tree->_nodeList + index;
    }

    /* Child node query
     * Note: return NULL when current node has no left/right child
     */
    inline BVNode* GetChildLeft() const {
        if(isLeaf()) return NULL;
        Int32 index = getNodeIndex() * 2 + 1;
        return _tree->_nodeList + index;
    }
    inline BVNode* GetChildRight() const {
        if(isLeaf()) return NULL;
        Int32 index = getNodeIndex() * 2 + 2;
        return _tree->_nodeList + index;
    }

    /* Update node state (BV and normal cone need to be update in this version) */
    void Update();

    /* Collision culling
     * Check whether two mesh region collide, which is useful for
     * speeding up collision detection.
     * Return TURE when NO collision we can guarantee, otherwise return FALSE.
     */
    inline Boolean CollisionCulling(const BVNode* another) const { return CollisionCulling(this, another); }
    inline static Boolean CollisionCulling(const BVNode* first, const BVNode* second) { return !first->_bv.Intersect(second->_bv); }

    /* Self collision culling
     * Check whether the mesh region has a self collision, which is useful for
     * speeding up collision detection.
     * Return TURE when NO self collision we can guarantee, otherwise return FALSE.
     * [2012-1-6: Currently, NO implemention here]
     * [2012-1-9: NormalCone culling added]
     */
    inline Boolean SelfCollisionCulling() const { return SelfCollisionCulling(this); }
    inline static Boolean SelfCollisionCulling(const BVNode* node) { return node->_nc.Validate(); }

private:
    /* The default constructor is hide */
    BVNode() {}

    /* The bounding volume */
    BV_AABB     _bv;

    /* The normal cone */
    NormalCone  _nc;

    /* The tree which node belongs to */
    BVTree*     _tree;

    /* Index query */
    inline Int32 getNodeIndex() const {
        Int32 res = (Int32)(this - _tree->_nodeList);
        BOOST_ASSERT(res >= 0 && res < _tree->_nodeNum);
        return res;
    }
    inline Boolean isLeaf() const {
        Int32 index = getNodeIndex() - (_tree->_nodeNum - _tree->_leafNum);
        return index >= 0 && index < _tree->_leafNum;
    }
    inline Int32 getLeafIndex() const {
        BOOST_ASSERT(isLeaf());
        return getNodeIndex() - (_tree->_nodeNum - _tree->_leafNum);
    }

    /* The friends */
    friend BVTree;
};

#pragma endregion

} // end of namespace geom
LLIB_END

#endif
